// TI File $Revision: /main/4 $
// Checkin $Date: April 18, 2005   10:04:09 $
//###########################################################################
//
// FILE:	Example_posspeed.c
//
// TITLE:	Pos/speed measurement using EQEP peripheral
//
// DESCRIPTION:
//
// This file contains source for the posspeed module
//
//###########################################################################
// Original Author: SD
//
// $TI Release: DSP280x V1.10 $
// $Release Date: April 18, 2005 $
//###########################################################################

#include "DSP280x_Device.h"     // DSP280x Headerfile Include File
#include "DSP280x_Examples.h"   // DSP280x Examples Include File
#include "Example_posspeed.h"   // Example specific Include file
#include "User_define.h"
void  POSSPEED_Init(void)
{
		//eQEP
		EQep1Regs.QUPRD=100;			// Unit Timer for 100Hz
		EQep1Regs.QDECCTL.bit.QSRC=00;		// QEP clock Inputs
		
		EQep1Regs.QEPCTL.bit.FREE_SOFT=2;
		EQep1Regs.QEPCTL.bit.PCRM=00;		// PCRM=00 mode
		EQep1Regs.QEPCTL.bit.UTE=1; 		// Unit Timeout Enable 
		EQep1Regs.QEPCTL.bit.QCLM=1; 		// Latch on unit time out
		EQep1Regs.QPOSMAX=0xffffffff;
		EQep1Regs.QEPCTL.bit.QPEN=1; 		// QEP enable
		
		EQep1Regs.QCAPCTL.bit.UPPS=5;   	// 1/32 for unit position 
		EQep1Regs.QCAPCTL.bit.CCPS=7;		// 1/128 for CAP clock
		EQep1Regs.QCAPCTL.bit.CEN=1; 		// QEP Capture Enable
		

        EALLOW;                       		// Enable EALLOW
        GpioCtrlRegs.GPAMUX2.bit.GPIO20 = 1;// Assuming GPIO20 is EQEP1A
        GpioCtrlRegs.GPAMUX2.bit.GPIO21 = 1;// Assuming GPIO21 is EQEP1B
        GpioCtrlRegs.GPAMUX2.bit.GPIO23 = 1;// Assuming GPIO23 is EQEP1I
        EDIS;                         		// Disable EALLOW
        

		//eQEP2
		EQep2Regs.QUPRD=100;			// Unit Timer for 100Hz //CLK = SYSCLK * QUPRD??
		EQep2Regs.QDECCTL.bit.QSRC=00;		// QEP clock Inputs
		
		EQep2Regs.QEPCTL.bit.FREE_SOFT=2;
		EQep2Regs.QEPCTL.bit.PCRM=00;		// PCRM=00 mode
		EQep2Regs.QEPCTL.bit.UTE=1; 		// Unit Timeout Enable 
		EQep2Regs.QEPCTL.bit.QCLM=1; 		// Latch on unit time out
		EQep2Regs.QPOSMAX=0xffffffff;
		EQep2Regs.QEPCTL.bit.QPEN=1; 		// QEP enable
		
		EQep2Regs.QCAPCTL.bit.UPPS=5;   	// 1/32 for unit position
		EQep2Regs.QCAPCTL.bit.CCPS=7;		// 1/128 for CAP clock
		EQep2Regs.QCAPCTL.bit.CEN=1; 		// QEP Capture Enable
		

        EALLOW;                       		// Enable EALLOW
        GpioCtrlRegs.GPAMUX2.bit.GPIO24 = 2;// Assuming GPIO24 is EQEP1A
        GpioCtrlRegs.GPAMUX2.bit.GPIO25 = 2;// Assuming GPIO25 is EQEP1B
        GpioCtrlRegs.GPAMUX2.bit.GPIO26 = 2;// Assuming GPIO26 is EQEP1I
        EDIS;         
}

void POSSPEED_Calc(POSSPEED *p1, POSSPEED *p2)
{
     long tmp;
     unsigned int pos16bval,temp1;
   	 _iq Tmp1,newp1,oldp1,newp2,oldp2;
   	 

	 /****************************eQEP1 **********************************/

     p1->DirectionQep = EQep1Regs.QEPSTS.bit.QDF;

	 pos16bval=(unsigned long)EQep1Regs.QPOSCNT;
     p1->theta_raw = pos16bval+ p1->cal_angle;
     tmp = (long)((long)p1->theta_raw*(long)p1->mech_scaler);  	// Q0*Q26 = Q26 
     
     p1->theta_raw = p1->theta_raw - 8191 * roll_l ;

     tmp &= 0x03FFF000;
     p1->theta_mech = (int)(tmp>>11);         		// Q26 -> Q15 
     p1->theta_mech &= 0x7FFF;
	     
     p1->theta_elec = p1->pole_pairs*p1->theta_mech;  // Q0*Q15 = Q15 
     p1->theta_elec &= 0x7FFF;

// Check an index occurrence
     if (EQep1Regs.QFLG.bit.IEL == 1)                   
     {  
    	p1->index_sync_flag = 0x00F0;
    	EQep1Regs.QCLR.bit.IEL=1;					// Clear interrupt flag
     }
      

// Check unit Time out-event for speed calculation:
// Unit Timer is configured for 100Hz in INIT function
//**** High Speed Calcultation using QEP Position counter ****//
	if(EQep1Regs.QFLG.bit.UTO==1)
	{ 
		/** Differentiator	**/
	 	pos16bval=(unsigned int)EQep1Regs.QPOSLAT;	// Latched POSCNT value		
     	tmp = (long)((long)pos16bval*(long)p1->mech_scaler);  	// Q0*Q26 = Q26 
     	tmp &= 0x03FFF000;
     	tmp = (int)(tmp>>11);         				// Q26 -> Q15 
     	tmp &= 0x7FFF;
		newp1=_IQ15toIQ(tmp);
		oldp1=p1->oldpos;

   		if (p1->DirectionQep==0)      				// POSCNT is counting down
   		{
    		if (newp1>oldp1)
      			Tmp1 = - (_IQ(1) - newp1 + oldp1);
    		else
      		Tmp1 = newp1 -oldp1;
   		}
   		else if (p1->DirectionQep==1)      			// POSCNT is counting up
   		{
    		if (newp1<oldp1)
      		Tmp1 = _IQ(1) + newp1 - oldp1;
    		else 
      		Tmp1 = newp1 - oldp1;
   		}

	   	if (Tmp1>_IQ(1))
	     	p1->Speed_fr = _IQ(1);
	   	else if (Tmp1<_IQ(-1))
	     	p1->Speed_fr = _IQ(-1);      
	   	else
	     	p1->Speed_fr = Tmp1;

		// Update the electrical angle
    	p1->oldpos = newp1;
     
		// Change motor speed from pu value to rpm value (Q15 -> Q0)
		// Q0 = Q0*GLOBAL_Q => _IQXmpy(), X = GLOBAL_Q
   		p1->SpeedRpm_fr = _IQmpy(p1->BaseRpm,p1->Speed_fr); 
		//=======================================
		
		EQep1Regs.QCLR.bit.UTO=1;					// Clear interrupt flag
	}	

//**** Low-speed computation using QEP capture counter ****//	
	if(EQep1Regs.QEPSTS.bit.UPEVNT==1)
	{
		if(EQep1Regs.QEPSTS.bit.COEF==0) // No Capture overflow
			temp1=(unsigned long)EQep1Regs.QCPRDLAT;
		else							// Capture overflow, saturate the result
			temp1=0xFFFF;
	
		p1->Speed_pr = _IQdiv(p1->SpeedScaler,temp1); 
		Tmp1=p1->Speed_pr;
	
		if (Tmp1>_IQ(1))
	 		p1->Speed_pr = _IQ(1);   
		else
	 		p1->Speed_pr = Tmp1;
	
		if (p1->DirectionQep==0) 
			p1->SpeedRpm_pr = -_IQmpy(p1->BaseRpm,p1->Speed_pr); 	// Q0 = Q0*GLOBAL_Q => _IQXmpy(), X = GLOBAL_Q
		else
			p1->SpeedRpm_pr = _IQmpy(p1->BaseRpm,p1->Speed_pr); 	// Q0 = Q0*GLOBAL_Q => _IQXmpy(), X = GLOBAL_Q

	
		EQep1Regs.QEPSTS.all=0x88;					// Clear Unit position event flag	
													// Clear overflow error flag
	}


	if((p1->theta_raw) >= (8191)){
		roll_l ++ ; 
   		(p1->theta_raw) = (p1->theta_raw) - ( 8191 ) ;
	}

	else if((p1->theta_raw)<= -8192 ){
		roll_l -- ; 
   		(p1->theta_raw) = (p1->theta_raw) + ( 8192 ) ;
	}



    /****************************end of eQEP1 **********************************/

 /****************************eQEP2 **********************************/

	p2->DirectionQep = EQep2Regs.QEPSTS.bit.QDF;

	 pos16bval=(unsigned int)EQep2Regs.QPOSCNT;
     p2->theta_raw = pos16bval+ p2->cal_angle;
     tmp = (long)((long)p2->theta_raw*(long)p2->mech_scaler);  	// Q0*Q26 = Q26 

     p2->theta_raw = p2->theta_raw - 8191 * roll ;	

     tmp &= 0x03FFF000;
     p2->theta_mech = (int)(tmp>>11);         		// Q26 -> Q15 
     p2->theta_mech &= 0x7FFF;
	     
     p2->theta_elec = p2->pole_pairs*p2->theta_mech;  // Q0*Q15 = Q15 
     p2->theta_elec &= 0x7FFF;

// Check an index occurrence
     if (EQep2Regs.QFLG.bit.IEL == 1)                   
     {  
    	p2->index_sync_flag = 0x00F0;
    	EQep2Regs.QCLR.bit.IEL=1;					// Clear interrupt flag
     }
      

// Check unit Time out-event for speed calculation:
// Unit Timer is configured for 100Hz in INIT function
//**** High Speed Calcultation using QEP Position counter ****//
	if(EQep2Regs.QFLG.bit.UTO==1)
	{ 
		/** Differentiator	**/
	 	pos16bval=(unsigned int)EQep2Regs.QPOSLAT;	// Latched POSCNT value		
     	tmp = (long)((long)pos16bval*(long)p2->mech_scaler);  	// Q0*Q26 = Q26 
     	tmp &= 0x03FFF000;
     	tmp = (int)(tmp>>11);         				// Q26 -> Q15 
     	tmp &= 0x7FFF;
		newp2=_IQ15toIQ(tmp);
		oldp2=p2->oldpos;

   		if (p2->DirectionQep==0)      				// POSCNT is counting down
   		{
    		if (newp2>oldp2)
      			Tmp1 = - (_IQ(1) - newp2 + oldp2);
    		else
      		Tmp1 = newp2 -oldp2;
   		}
   		else if (p2->DirectionQep==1)      			// POSCNT is counting up
   		{
    		if (newp2<oldp2)
      		Tmp1 = _IQ(1) + newp2 - oldp2;
    		else 
      		Tmp1 = newp2 - oldp2;
   		}

	   	if (Tmp1>_IQ(1))
	     	p2->Speed_fr = _IQ(1);
	   	else if (Tmp1<_IQ(-1))
	     	p2->Speed_fr = _IQ(-1);      
	   	else
	     	p2->Speed_fr = Tmp1;

		// Update the electrical angle
    	p2->oldpos = newp2;
     
		// Change motor speed from pu value to rpm value (Q15 -> Q0)
		// Q0 = Q0*GLOBAL_Q => _IQXmpy(), X = GLOBAL_Q
   		p2->SpeedRpm_fr = _IQmpy(p2->BaseRpm,p2->Speed_fr); 
		//=======================================
		
		EQep2Regs.QCLR.bit.UTO=1;					// Clear interrupt flag
	}	

//**** Low-speed computation using QEP capture counter ****//	
	if(EQep2Regs.QEPSTS.bit.UPEVNT==1)
	{
		if(EQep2Regs.QEPSTS.bit.COEF==0) // No Capture overflow
			temp1=(unsigned long)EQep2Regs.QCPRDLAT;
		else							// Capture overflow, saturate the result
			temp1=0xFFFF;
	
		p2->Speed_pr = _IQdiv(p2->SpeedScaler,temp1); 
		Tmp1=p2->Speed_pr;
	
		if (Tmp1>_IQ(1))
	 		p2->Speed_pr = _IQ(1);   
		else
	 		p2->Speed_pr = Tmp1;
	
		if (p2->DirectionQep==0) 
			p2->SpeedRpm_pr = -_IQmpy(p2->BaseRpm,p2->Speed_pr); 	// Q0 = Q0*GLOBAL_Q => _IQXmpy(), X = GLOBAL_Q
		else
			p2->SpeedRpm_pr = _IQmpy(p2->BaseRpm,p2->Speed_pr); 	// Q0 = Q0*GLOBAL_Q => _IQXmpy(), X = GLOBAL_Q

	
		EQep2Regs.QEPSTS.all=0x88;					// Clear Unit position event flag	
													// Clear overflow error flag
	}

	if((p2->theta_raw) >= (8191)){
		roll ++ ; 
   		(p2->theta_raw) = (p2->theta_raw) - ( 8191 ) ;
	}

	else if((p2->theta_raw)<= -8192 ){
		roll -- ; 
   		(p2->theta_raw) = (p2->theta_raw) + ( 8192 ) ;
	}

	/*if(8191*2+1>(p2->theta_raw) >= 8191)
   		{ roll ++; 
   		(p2->theta_raw) = (p2->theta_raw) - 8191 ;
      	}
	else if((p2->theta_raw) >= 8191*2+1)
   		{ roll ++; 
   		(p2->theta_raw) = (p2->theta_raw) - (8191 * 2+1) ;
      	
	 	}
 	else if(-8192*2<(p2->theta_raw) <= -8192)
   		{ (p2->theta_raw) = (p2->theta_raw) + 8192;
      	  roll -- ;
	 	}
	else if((p2->theta_raw) <= -8192*2)
  		{ (p2->theta_raw) = (p2->theta_raw) + 8192*2;
      	  roll -- ;
	 	}*/
 /****************************end of eQEP1 **********************************/
}


